"""
!/usr/bin/env python
-*- coding: utf-8 -*-
@CreateTime : 2024/11/14 10:51
@Author  :    AnimateX
@Contact :    animatex@163.com
@File    :    rmsl_precison_test.py
@License :    Copyright © 2024 AnimateX. All rights reserved.
@Version :    rmsl_precison_test_2024/11/14.0.1

-------------------------------------------------------------------------------
# @Description:

    ---------------------------------------------------------------------------
    [Update History]:
        2024/11/14:

-------------------------------------------------------------------------------
"""
import os
import cv2
import numpy as np
from tqdm import tqdm
from natsort import natsorted
from precision_core import RMSLPrecision
from parser_cam_param import ParserCamParam
from utils import Utils


def get_rmsl_cam_param_from_yaml(yaml_file_path: str) -> dict:
    if not os.path.exists(yaml_file_path):
        raise FileNotFoundError(f' [Error] File {yaml_file_path} not found.')
    fs = cv2.FileStorage(yaml_file_path, cv2.FILE_STORAGE_READ)

    # Image dimensions
    rgb_w = int(fs.getNode("rgb_width").real())
    rgb_h = int(fs.getNode("rgb_height").real())
    ir_l_w = int(fs.getNode("l_width").real())
    ir_l_h = int(fs.getNode("l_height").real())
    ir_r_w = int(fs.getNode("r_width").real())
    ir_r_h = int(fs.getNode("r_height").real())

    # RGB Camera intrinsics
    k_rgb = fs.getNode("K_rgb").mat()
    d_rgb = fs.getNode("D_rgb").mat()
    r_ir2rgb = fs.getNode("R_ir2rgb").mat()
    t_ir2rgb = fs.getNode("T_ir2rgb").mat()

    # ir camera intrinsics
    k_ir_l = fs.getNode("K_l").mat()
    d_ir_l = fs.getNode("D_l").mat()
    k_ir_r = fs.getNode("K_r").mat()
    d_ir_r = fs.getNode("D_r").mat()

    # Stereo rectification parameters
    r1 = fs.getNode("R1").mat()
    r2 = fs.getNode("R2").mat()
    p1 = fs.getNode("P1").mat()
    p2 = fs.getNode("P2").mat()
    q_mat = fs.getNode("Q").mat()

    inv_pr1 = fs.getNode("invPR1").mat()
    inv_pr2 = fs.getNode("invPR2").mat()

    fs.release()

    # 提取 K_l 和 K_rgb 中的参数
    fx_ir = k_ir_l[0, 0]
    fy_ir = k_ir_l[1, 1]
    cx_ir = k_ir_l[0, 2]
    cy_ir = k_ir_l[1, 2]

    fx_rgb = k_rgb[0, 0]
    fy_rgb = k_rgb[1, 1]
    cx_rgb = k_rgb[0, 2]
    cy_rgb = k_rgb[1, 2]

    # 构建 K_ir 和 K_rgb 的 3x3 矩阵
    mat_k_ir = np.array([
        [fx_ir, 0, cx_ir],
        [0, fy_ir, cy_ir],
        [0, 0, 1]
    ], dtype=np.float32)

    mat_k_rgb = np.array([
        [fx_rgb, 0, cx_rgb],
        [0, fy_rgb, cy_rgb],
        [0, 0, 1]
    ], dtype=np.float32)

    # 构建 invPR 和 ratio2 矩阵
    mat_inv_pr = np.array([
        [inv_pr1[0, 0], inv_pr1[0, 1], inv_pr1[0, 2]],
        [inv_pr1[1, 0], inv_pr1[1, 1], inv_pr1[1, 2]],
        [inv_pr1[2, 0], inv_pr1[2, 1], inv_pr1[2, 2]]
    ], dtype=np.float32)

    ratio2 = np.eye(3, dtype=np.float32)  # 3x3 单位矩阵
    mat_inv_pr = mat_inv_pr @ ratio2  # invPR * ratio2

    # 构建 R_ir2rgb 和 T_ir2rgb
    mat_r_ir2rgb = np.array([
        [r_ir2rgb[0, 0], r_ir2rgb[0, 1], r_ir2rgb[0, 2]],
        [r_ir2rgb[1, 0], r_ir2rgb[1, 1], r_ir2rgb[1, 2]],
        [r_ir2rgb[2, 0], r_ir2rgb[2, 1], r_ir2rgb[2, 2]]
    ], dtype=np.float32)

    mat_t_ir2rgb = np.array([t_ir2rgb[0, 0], t_ir2rgb[1, 0], t_ir2rgb[2, 0]], dtype=np.float32)

    # 计算 R 和 T
    mat_r = mat_k_rgb @ mat_r_ir2rgb @ mat_inv_pr
    vec_t = mat_k_rgb @ mat_t_ir2rgb

    q23 = float(q_mat[2, 3])
    q32 = float(q_mat[3, 2])
    q33 = float(q_mat[3, 3])

    q_coeffs = {'Q23': q23, 'Q32': q32, 'Q33': q33}
    r_coeffs = {
        'r0': mat_r[0, 0],
        'r1': mat_r[0, 1],
        'r2': mat_r[0, 2],
        'r3': mat_r[1, 0],
        'r4': mat_r[1, 1],
        'r5': mat_r[1, 2],
        'r6': mat_r[2, 0],
        'r7': mat_r[2, 1],
        'r8': mat_r[2, 2]
    }
    t_coeffs = {
        't0': vec_t[0],
        't1': vec_t[1],
        't2': vec_t[2]
    }

    color_intrinsics = {
        'fx': fx_rgb,
        'fy': fy_rgb,
        'cx': cx_rgb,
        'cy': cy_rgb,
        'width': rgb_w,
        'height': rgb_h
    }

    depth_intrinsics = {
        'fx': fx_ir,
        'fy': fy_ir,
        'cx': cx_ir,
        'cy': cy_ir,
        'width': ir_l_w,
        'height': ir_l_h
    }

    cam_param = {
        'color_intrinsics': color_intrinsics,
        'depth_intrinsics': depth_intrinsics,
        'r_coeffs': r_coeffs,
        't_coeffs': t_coeffs,
        'q_coeffs': q_coeffs
    }

    return cam_param


def preprocess(yaml_file_path: str, test_precision_dir: str, key='_depth.png', en_disp=False, en_binning=True):
    if not os.path.exists(yaml_file_path):
        print(' [ERROR] Check yaml file path.')
        return

    if not os.path.exists(test_precision_dir):
        print(' [ERROR] Test directory is empty')
        return
    cam_param = get_rmsl_cam_param_from_yaml(yaml_file_path)

    test_img_name_list = os.listdir(test_precision_dir)
    if en_disp:
        key = '_disp'

    img_name_list = [img_name for img_name in test_img_name_list if '.png' in img_name and '_align' not in img_name]
    img_name_list = [img_name for img_name in img_name_list if key in img_name]
    img_path_list = [os.path.join(test_precision_dir, img_name) for img_name in img_name_list]
    img_path_list = natsorted(img_path_list)

    if en_disp:
        dp_img_path_list = []
        for idx, disp_img_path in enumerate(tqdm(img_path_list, desc="Disp to Depth...", colour='cyan')):
            dp_img_path = disp_img_path.replace('_disp', '_depth')
            disp_img = cv2.imread(disp_img_path, cv2.IMREAD_UNCHANGED)

            if en_binning:
                # 640x416 --> 640x540
                disp_img = np.pad(disp_img, ((62, 62), (0, 0)), mode='constant', constant_values=0)

            dp_img = Utils().disp_to_depth(disp_img,
                                           cam_param['q_coeffs']['Q23'],
                                           cam_param['q_coeffs']['Q32'],
                                           cam_param['q_coeffs']['Q33'],
                                           sub_pixel_value=64,
                                           zoom_ratio=1.0,
                                           max_dis=3000)
            cv2.imwrite(dp_img_path, dp_img)
            dp_img_path_list.append(dp_img_path)

        return cam_param, dp_img_path_list
    else:
        return cam_param, img_path_list


def cal_precision(depth_img_path_list: list, cam_info: dict, csv_file_name: str, measureDelta=0, en_binning=True):
    """

    :param en_binning:
    :param depth_img_path_list:
    :param cam_info:
    :param csv_file_name:
    :param measureDelta:            ob 测量的距离实际要加上这个delta
    :return:
    """
    if len(depth_img_path_list) == 0:
        print(' [ERROR] List is empty!')
        return

    precision = RMSLPrecision()
    all_result_dict = {}

    depth_img_path_list = natsorted(depth_img_path_list)
    for idx, dp_img_path in enumerate(tqdm(depth_img_path_list, desc=' Processing (cal-precision)...', colour='cyan')):
        # dp_img_path = ("/home/dataset_b/rmslTestDataset/orbbec/gemini335-01/v0619/r1920x1080_d1280x800/"
        #                "precision/normal-precision/0008_indoor_white_wall_depth_200mm.png")

        dp_img_name = dp_img_path.split('/')[-1]
        measured_distance = int(dp_img_name.split('_')[0].replace('mm', ''))
        measured_distance += measureDelta
        depth_intrinsic = cam_info['depth_intrinsics']
        if depth_intrinsic is None or len(depth_intrinsic) == 0:
            depth_intrinsic = {
                'fx': 622.810913,
                'fy': 622.810913,
                'cx': 634.000000,
                'cy': 397.000000,
                'width': 1280,
                'height': 800
            }

        dp_img = Utils().readImage(dp_img_path,
                                   depth_intrinsic['width'],
                                   depth_intrinsic['height'],
                                   data_type=np.uint16,
                                   mode='depth')

        hole_rate = Utils().calculate_hole_rate(dp_img)
        if hole_rate > 0.5:
            print(f" [ERROR] Image hole rate too high: {hole_rate * 100:5.2f}%")
            continue

        if en_binning:
            # 640x416 --> 640x540
            dp_img = np.pad(dp_img, ((62, 62), (0, 0)), mode='constant', constant_values=0)

        # Use image center point not OpticalCenter
        abs_acc_res = precision.calMeanPrecisionByORBBECMethod(dp_img,
                                                               measured_distance,
                                                               depth_intrinsic['cx'],
                                                               depth_intrinsic['cy'],
                                                               roi_w=24,
                                                               roi_h=24,
                                                               cen_size_h=24,
                                                               cen_size_v=24,
                                                               min_ratio=0.5,
                                                               max_ratio=0.9,
                                                               en_img_cen=True,
                                                               en_precise_acc=False)

        # Use image center
        rel_acc_res = precision.calRelativePrecision(dp_img,
                                                     depth_intrinsic['fx'],
                                                     depth_intrinsic['fy'],
                                                     depth_intrinsic['cx'],
                                                     depth_intrinsic['cy'],
                                                     seed=42,
                                                     cen_size_h=24,
                                                     cen_size_v=24,
                                                     min_ratio=0.5,
                                                     max_ratio=0.9,
                                                     use_img_cen=True,
                                                     en_filter=True)

        ecovacs_abs_acc = precision.calEcovasRobotPointCloudPrecision(dp_img,
                                                                      measured_distance,
                                                                      depth_intrinsic['fx'],
                                                                      depth_intrinsic['fy'],
                                                                      depth_intrinsic['cx'],
                                                                      depth_intrinsic['cy'])

        ecovacs_rel_acc = precision.calEcovasRobotPointCloudPrecisionByFit(dp_img,
                                                                           depth_intrinsic['fx'],
                                                                           depth_intrinsic['fy'],
                                                                           depth_intrinsic['cx'],
                                                                           depth_intrinsic['cy'])

        result_dict = {'distance': (measured_distance - measureDelta), 'hole_rate': hole_rate,
                       'outlier_percent': rel_acc_res['outlier_percent'],
                       'abs_avg_roi_c': abs_acc_res['avg_roi_c'],
                       'abs_avg_roi_l': abs_acc_res['avg_roi_l'], 'abs_avg_roi_r': abs_acc_res['avg_roi_r'],
                       'abs_avg_roi_t': abs_acc_res['avg_roi_t'], 'abs_avg_roi_b': abs_acc_res['avg_roi_b'],
                       'abs_avg_roi_25': abs_acc_res['avg_roi_25'], 'abs_avg_roi_81': abs_acc_res['avg_roi_81'],
                       'abs_avg_roi_cen': abs_acc_res['avg_roi_cen'], 'abs_min_roi_c': abs_acc_res['min_roi_c'],
                       'abs_min_roi_l': abs_acc_res['min_roi_l'], 'abs_min_roi_r': abs_acc_res['min_roi_r'],
                       'abs_min_roi_t': abs_acc_res['min_roi_t'], 'abs_min_roi_b': abs_acc_res['min_roi_b'],
                       'abs_min_roi_25': abs_acc_res['min_roi_25'], 'abs_min_roi_81': abs_acc_res['min_roi_81'],
                       'abs_min_roi_cen': abs_acc_res['min_roi_cen'], 'abs_max_roi_c': abs_acc_res['max_roi_c'],
                       'abs_max_roi_l': abs_acc_res['max_roi_l'], 'abs_max_roi_r': abs_acc_res['max_roi_r'],
                       'abs_max_roi_t': abs_acc_res['max_roi_t'], 'abs_max_roi_b': abs_acc_res['max_roi_b'],
                       'abs_max_roi_25': abs_acc_res['max_roi_25'], 'abs_max_roi_81': abs_acc_res['max_roi_81'],
                       'abs_max_roi_cen': abs_acc_res['max_roi_cen'], 'abs_std_roi_c': abs_acc_res['std_roi_c'],
                       'abs_std_roi_l': abs_acc_res['std_roi_l'], 'abs_std_roi_r': abs_acc_res['std_roi_r'],
                       'abs_std_roi_t': abs_acc_res['std_roi_t'], 'abs_std_roi_b': abs_acc_res['std_roi_b'],
                       'abs_std_roi_25': abs_acc_res['std_roi_25'], 'abs_std_roi_81': abs_acc_res['std_roi_81'],
                       'abs_std_roi_cen': abs_acc_res['std_roi_cen'], 'abs_var_roi_c': abs_acc_res['var_roi_c'],
                       'abs_var_roi_l': abs_acc_res['var_roi_l'], 'abs_var_roi_r': abs_acc_res['var_roi_r'],
                       'abs_var_roi_t': abs_acc_res['var_roi_t'], 'abs_var_roi_b': abs_acc_res['var_roi_b'],
                       'abs_var_roi_25': abs_acc_res['var_roi_25'], 'abs_var_roi_81': abs_acc_res['var_roi_81'],
                       'abs_var_roi_cen': abs_acc_res['var_roi_cen'], 'rmse_roi_c': abs_acc_res['rmse_roi_c'],
                       'rmse_roi_l': abs_acc_res['rmse_roi_l'], 'rmse_roi_r': abs_acc_res['rmse_roi_r'],
                       'rmse_roi_t': abs_acc_res['rmse_roi_t'], 'rmse_roi_b': abs_acc_res['rmse_roi_b'],
                       'rmse_roi_25': abs_acc_res['rmse_roi_25'], 'rmse_roi_81': abs_acc_res['rmse_roi_81'],
                       'rmse_roi_cen': abs_acc_res['rmse_roi_cen'],
                       'rel_fov_h_val': rel_acc_res['fov_h_val'], 'rel_fov_h_max': rel_acc_res['fov_h_max'],
                       'rel_fov_h_min': rel_acc_res['fov_h_min'], 'rel_fov_h_cen': rel_acc_res['fov_h_cen'],
                       'rel_fov_v_val': rel_acc_res['fov_v_val'], 'rel_fov_v_max': rel_acc_res['fov_v_max'],
                       'rel_fov_v_min': rel_acc_res['fov_v_min'], 'rel_fov_v_cen': rel_acc_res['fov_v_cen'],
                       'rel_avg_roi_25': rel_acc_res['avg_roi_25'], 'rel_avg_roi_81': rel_acc_res['avg_roi_81'],
                       'rel_avg_roi_cen': rel_acc_res['avg_roi_cen'], 'rel_min_roi_25': rel_acc_res['min_roi_25'],
                       'rel_min_roi_81': rel_acc_res['min_roi_81'], 'rel_min_roi_cen': rel_acc_res['min_roi_cen'],
                       'rel_max_roi_25': rel_acc_res['max_roi_25'], 'rel_max_roi_81': rel_acc_res['max_roi_81'],
                       'rel_max_roi_cen': rel_acc_res['max_roi_cen'], 'rel_std_roi_25': rel_acc_res['std_roi_25'],
                       'rel_std_roi_81': rel_acc_res['std_roi_81'], 'rel_std_roi_cen': rel_acc_res['std_roi_cen'],
                       'rel_var_roi_25': rel_acc_res['var_roi_25'], 'rel_var_roi_81': rel_acc_res['var_roi_81'],
                       'rel_var_roi_cen': rel_acc_res['var_roi_cen'],
                       'rel_avg_per_roi_25': rel_acc_res['avg_per_roi_25'],
                       'rel_avg_per_roi_81': rel_acc_res['avg_per_roi_81'],
                       'rel_avg_per_roi_cen': rel_acc_res['avg_per_roi_cen'],
                       'rel_min_per_roi_25': rel_acc_res['min_per_roi_25'],
                       'rel_min_per_roi_81': rel_acc_res['min_per_roi_81'],
                       'rel_min_per_roi_cen': rel_acc_res['min_per_roi_cen'],
                       'rel_max_per_roi_25': rel_acc_res['max_per_roi_25'],
                       'rel_max_per_roi_81': rel_acc_res['max_per_roi_81'],
                       'rel_max_per_roi_cen': rel_acc_res['max_per_roi_cen'],
                       'rel_std_per_roi_25': rel_acc_res['std_per_roi_25'],
                       'rel_std_per_roi_81': rel_acc_res['std_per_roi_81'],
                       'rel_std_per_roi_cen': rel_acc_res['std_per_roi_cen'],
                       'rel_var_per_roi_25': rel_acc_res['var_per_roi_25'],
                       'rel_var_per_roi_81': rel_acc_res['var_per_roi_81'],
                       'rel_var_per_roi_cen': rel_acc_res['var_per_roi_cen'], 'abs_LU_avg': ecovacs_abs_acc['LU_avg'],
                       'abs_CU_avg': ecovacs_abs_acc['CU_avg'], 'abs_RU_avg': ecovacs_abs_acc['RU_avg'],
                       'abs_LC_avg': ecovacs_abs_acc['LC_avg'], 'abs_CC_avg': ecovacs_abs_acc['CC_avg'],
                       'abs_RC_avg': ecovacs_abs_acc['RC_avg'], 'abs_LD_avg': ecovacs_abs_acc['LD_avg'],
                       'abs_CD_avg': ecovacs_abs_acc['CD_avg'], 'abs_RD_avg': ecovacs_abs_acc['RD_avg'],
                       'abs_LU_min': ecovacs_abs_acc['LU_min'], 'abs_CU_min': ecovacs_abs_acc['CU_min'],
                       'abs_RU_min': ecovacs_abs_acc['RU_min'], 'abs_LC_min': ecovacs_abs_acc['LC_min'],
                       'abs_CC_min': ecovacs_abs_acc['CC_min'], 'abs_RC_min': ecovacs_abs_acc['RC_min'],
                       'abs_LD_min': ecovacs_abs_acc['LD_min'], 'abs_CD_min': ecovacs_abs_acc['CD_min'],
                       'abs_RD_min': ecovacs_abs_acc['RD_min'], 'abs_LU_max': ecovacs_abs_acc['LU_max'],
                       'abs_CU_max': ecovacs_abs_acc['CU_max'], 'abs_RU_max': ecovacs_abs_acc['RU_max'],
                       'abs_LC_max': ecovacs_abs_acc['LC_max'], 'abs_CC_max': ecovacs_abs_acc['CC_max'],
                       'abs_RC_max': ecovacs_abs_acc['RC_max'], 'abs_LD_max': ecovacs_abs_acc['LD_max'],
                       'abs_CD_max': ecovacs_abs_acc['CD_max'], 'abs_RD_max': ecovacs_abs_acc['RD_max'],
                       'abs_LU_std': ecovacs_abs_acc['LU_std'], 'abs_CU_std': ecovacs_abs_acc['CU_std'],
                       'abs_RU_std': ecovacs_abs_acc['RU_std'], 'abs_LC_std': ecovacs_abs_acc['LC_std'],
                       'abs_CC_std': ecovacs_abs_acc['CC_std'], 'abs_RC_std': ecovacs_abs_acc['RC_std'],
                       'abs_LD_std': ecovacs_abs_acc['LD_std'], 'abs_CD_std': ecovacs_abs_acc['CD_std'],
                       'abs_RD_std': ecovacs_abs_acc['RD_std'], 'abs_LU_var': ecovacs_abs_acc['LU_var'],
                       'abs_CU_var': ecovacs_abs_acc['CU_var'], 'abs_RU_var': ecovacs_abs_acc['RU_var'],
                       'abs_LC_var': ecovacs_abs_acc['LC_var'], 'abs_CC_var': ecovacs_abs_acc['CC_var'],
                       'abs_RC_var': ecovacs_abs_acc['RC_var'], 'abs_LD_var': ecovacs_abs_acc['LD_var'],
                       'abs_CD_var': ecovacs_abs_acc['CD_var'], 'abs_RD_var': ecovacs_abs_acc['RD_var'],
                       'rel_LU_avg': ecovacs_rel_acc['LU_avg'], 'rel_CU_avg': ecovacs_rel_acc['CU_avg'],
                       'rel_RU_avg': ecovacs_rel_acc['RU_avg'], 'rel_LC_avg': ecovacs_rel_acc['LC_avg'],
                       'rel_CC_avg': ecovacs_rel_acc['CC_avg'], 'rel_RC_avg': ecovacs_rel_acc['RC_avg'],
                       'rel_LD_avg': ecovacs_rel_acc['LD_avg'], 'rel_CD_avg': ecovacs_rel_acc['CD_avg'],
                       'rel_RD_avg': ecovacs_rel_acc['RD_avg'], 'rel_LU_min': ecovacs_rel_acc['LU_min'],
                       'rel_CU_min': ecovacs_rel_acc['CU_min'], 'rel_RU_min': ecovacs_rel_acc['RU_min'],
                       'rel_LC_min': ecovacs_rel_acc['LC_min'], 'rel_CC_min': ecovacs_rel_acc['CC_min'],
                       'rel_RC_min': ecovacs_rel_acc['RC_min'], 'rel_LD_min': ecovacs_rel_acc['LD_min'],
                       'rel_CD_min': ecovacs_rel_acc['CD_min'], 'rel_RD_min': ecovacs_rel_acc['RD_min'],
                       'rel_LU_max': ecovacs_rel_acc['LU_max'], 'rel_CU_max': ecovacs_rel_acc['CU_max'],
                       'rel_RU_max': ecovacs_rel_acc['RU_max'], 'rel_LC_max': ecovacs_rel_acc['LC_max'],
                       'rel_CC_max': ecovacs_rel_acc['CC_max'], 'rel_RC_max': ecovacs_rel_acc['RC_max'],
                       'rel_LD_max': ecovacs_rel_acc['LD_max'], 'rel_CD_max': ecovacs_rel_acc['CD_max'],
                       'rel_RD_max': ecovacs_rel_acc['RD_max'], 'rel_LU_std': ecovacs_rel_acc['LU_std'],
                       'rel_CU_std': ecovacs_rel_acc['CU_std'], 'rel_RU_std': ecovacs_rel_acc['RU_std'],
                       'rel_LC_std': ecovacs_rel_acc['LC_std'], 'rel_CC_std': ecovacs_rel_acc['CC_std'],
                       'rel_RC_std': ecovacs_rel_acc['RC_std'], 'rel_LD_std': ecovacs_rel_acc['LD_std'],
                       'rel_CD_std': ecovacs_rel_acc['CD_std'], 'rel_RD_std': ecovacs_rel_acc['RD_std'],
                       'rel_LU_var': ecovacs_rel_acc['LU_var'], 'rel_CU_var': ecovacs_rel_acc['CU_var'],
                       'rel_RU_var': ecovacs_rel_acc['RU_var'], 'rel_LC_var': ecovacs_rel_acc['LC_var'],
                       'rel_CC_var': ecovacs_rel_acc['CC_var'], 'rel_RC_var': ecovacs_rel_acc['RC_var'],
                       'rel_LD_var': ecovacs_rel_acc['LD_var'], 'rel_CD_var': ecovacs_rel_acc['CD_var'],
                       'rel_RD_var': ecovacs_rel_acc['RD_var']}
        all_result_dict[measured_distance - measureDelta] = result_dict

    headers = ['distance', 'hole_rate', 'outlier_percent',
               'rmse_roi_c', 'rmse_roi_l', 'rmse_roi_r', 'rmse_roi_t', 'rmse_roi_b', 'rmse_roi_25', 'rmse_roi_81', 'rmse_roi_cen',
               'abs_avg_roi_c', 'abs_avg_roi_l', 'abs_avg_roi_r', 'abs_avg_roi_t', 'abs_avg_roi_b', 'abs_avg_roi_25', 'abs_avg_roi_81', 'abs_avg_roi_cen',
               'abs_min_roi_c', 'abs_min_roi_l', 'abs_min_roi_r', 'abs_min_roi_t', 'abs_min_roi_b', 'abs_min_roi_25', 'abs_min_roi_81', 'abs_min_roi_cen',
               'abs_max_roi_c', 'abs_max_roi_l', 'abs_max_roi_r', 'abs_max_roi_t', 'abs_max_roi_b', 'abs_max_roi_25', 'abs_max_roi_81', 'abs_max_roi_cen',
               'abs_std_roi_c', 'abs_std_roi_l', 'abs_std_roi_r', 'abs_std_roi_t', 'abs_std_roi_b', 'abs_std_roi_25', 'abs_std_roi_81', 'abs_std_roi_cen',
               'abs_var_roi_c', 'abs_var_roi_l', 'abs_var_roi_r', 'abs_var_roi_t', 'abs_var_roi_b', 'abs_var_roi_25', 'abs_var_roi_81', 'abs_var_roi_cen',
               'rel_fov_h_val', 'rel_fov_h_max', 'rel_fov_h_min', 'rel_fov_h_cen', 'rel_fov_v_val', 'rel_fov_v_max', 'rel_fov_v_min', 'rel_fov_v_cen',
               'rel_avg_roi_25', 'rel_avg_roi_81', 'rel_avg_roi_cen',
               'rel_min_roi_25', 'rel_min_roi_81', 'rel_min_roi_cen',
               'rel_max_roi_25', 'rel_max_roi_81', 'rel_max_roi_cen',
               'rel_std_roi_25', 'rel_std_roi_81', 'rel_std_roi_cen',
               'rel_var_roi_25', 'rel_var_roi_81', 'rel_var_roi_cen',
               'rel_avg_per_roi_25', 'rel_avg_per_roi_81', 'rel_avg_per_roi_cen',
               'rel_min_per_roi_25', 'rel_min_per_roi_81', 'rel_min_per_roi_cen',
               'rel_max_per_roi_25', 'rel_max_per_roi_81', 'rel_max_per_roi_cen',
               'rel_std_per_roi_25', 'rel_std_per_roi_81', 'rel_std_per_roi_cen',
               'rel_var_per_roi_25', 'rel_var_per_roi_81', 'rel_var_per_roi_cen',
               'abs_LU_avg', 'abs_CU_avg', 'abs_RU_avg', 'abs_LC_avg', 'abs_CC_avg', 'abs_RC_avg', 'abs_LD_avg', 'abs_CD_avg', 'abs_RD_avg',
               'abs_LU_min', 'abs_CU_min', 'abs_RU_min', 'abs_LC_min', 'abs_CC_min', 'abs_RC_min', 'abs_LD_min', 'abs_CD_min', 'abs_RD_min',
               'abs_LU_max', 'abs_CU_max', 'abs_RU_max', 'abs_LC_max', 'abs_CC_max', 'abs_RC_max', 'abs_LD_max', 'abs_CD_max', 'abs_RD_max',
               'abs_LU_std', 'abs_CU_std', 'abs_RU_std', 'abs_LC_std', 'abs_CC_std', 'abs_RC_std', 'abs_LD_std', 'abs_CD_std', 'abs_RD_std',
               'abs_LU_var', 'abs_CU_var', 'abs_RU_var', 'abs_LC_var', 'abs_CC_var', 'abs_RC_var', 'abs_LD_var', 'abs_CD_var', 'abs_RD_var',
               'rel_LU_avg', 'rel_CU_avg', 'rel_RU_avg', 'rel_LC_avg', 'rel_CC_avg', 'rel_RC_avg', 'rel_LD_avg', 'rel_CD_avg', 'rel_RD_avg',
               'rel_LU_min', 'rel_CU_min', 'rel_RU_min', 'rel_LC_min', 'rel_CC_min', 'rel_RC_min', 'rel_LD_min', 'rel_CD_min', 'rel_RD_min',
               'rel_LU_max', 'rel_CU_max', 'rel_RU_max', 'rel_LC_max', 'rel_CC_max', 'rel_RC_max', 'rel_LD_max', 'rel_CD_max', 'rel_RD_max',
               'rel_LU_std', 'rel_CU_std', 'rel_RU_std', 'rel_LC_std', 'rel_CC_std', 'rel_RC_std', 'rel_LD_std', 'rel_CD_std', 'rel_RD_std',
               'rel_LU_var', 'rel_CU_var', 'rel_RU_var', 'rel_LC_var', 'rel_CC_var', 'rel_RC_var', 'rel_LD_var', 'rel_CD_var', 'rel_RD_var']

    Utils().saveDictToCsv(all_result_dict, csv_file_name, headers)


if __name__ == '__main__':
    alg_version = 'rmsl_20241112'
    enable_binning = True
    if enable_binning:
        rmsl_yaml_cfg_path = '/home/dataset_b/rmslTestDataset/rockchip/rmsl321/v241111/calib_param_binning.yaml'
    else:
        rmsl_yaml_cfg_path = '/home/dataset_b/rmslTestDataset/rockchip/rmsl321/v241111/calib_param.yaml'

    test_dir = '/home/dataset_b/rmslTestDataset/rockchip/rmsl321/v241111/precision'
    save_csv_file_name = f"/home/dataset_b/rmslTestDataset/rockchip/rmsl321/v241111/TestRes/{alg_version}_rk3588_float_precision_result.csv"

    camera_param, depth_image_path_list = preprocess(rmsl_yaml_cfg_path, test_dir, key='_depth', en_binning=enable_binning)
    cal_precision(depth_image_path_list, camera_param, save_csv_file_name, enable_binning)
